%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This Matlab program is for Figure 1 from the Supplemental Appendix of 'The explicit formula for the Hodrick-Prescott filter in finite sample' by Adriana Cornea-Madeira
% Kalman filter plus smoothing

clear all; clc;
nnn = (100:100:10000)';
Kk = 50; %number of samples
t = zeros(length(nnn),1);
parfor in=1:length(nnn)
tic
n = nnn(in,1); 
for k = 1:Kk
% local linear trend model (LLTM)
beta = zeros(n,1);
tau = zeros(n,1);
y = zeros(n,1);
sigma_c = 2;
sigma_eta = 0; %smooth trend model
sigma_zeta = 0.025*sigma_c; 
beta(1) = randn(1,1); tau(1) = randn(1,1); y(1) = randn(1,1);
epsi_1 = randn(n,1); epsi_2 = randn(n,1); epsi_3 = randn(n,1);
for i = 2:n
    beta(i) = beta(i-1) + sigma_zeta*epsi_1(i,1);
    tau(i) = tau(i-1) + beta(i-1) + sigma_eta*epsi_2(i,1);
    y(i) = tau(i) + sigma_c*epsi_3(i,1);
end

%system matrices
z = [1 0]; S = [1 1;0 1]; h = [sigma_c 0 0]; H = [0 sigma_eta 0;0 0 sigma_zeta];

q_tau = sigma_eta^2/sigma_c^2; lambda = sigma_c^2/sigma_zeta^2; q_beta=1/lambda;
% usual Kalman filter starts at t=3 with: tilde_mu(5:6) and Sigma(5:6,:)
v = zeros(n,1); l = zeros(2*n,1); sigma = zeros(n,1); Sigma = zeros(2*n,2); tilde_mu=zeros(2*n,1);
Sigma(5:6,:) = sigma_c^2*[5+2*q_tau+q_beta 3+q_tau+q_beta;3+q_tau+q_beta 2+q_tau+2*q_beta];
tilde_mu(5:6,1) = [2*y(2)-y(1);y(2)-y(1)];

v(3) = y(3) - z*tilde_mu(5:6,:);
sigma(3) = z*Sigma(5:6,:)*z'+h*h';
l(5:6,1) = S*Sigma(5:6,:)*z';

%MMSE estimator and its corresponding variance
for i=4:n
    tilde_mu((2*i-1):2*i,1) = S*tilde_mu((2*i-3):(2*i-2),1) + l((2*i-3):(2*i-2),1)*v(i-1)/sigma(i-1); % E(tilde_mu_i+1|y_1,...,y_i)
    Sigma((2*i-1):2*i,:) = S*Sigma((2*i-3):(2*i-2),:)*S'+H*H'-l((2*i-3):(2*i-2),1)*l((2*i-3):(2*i-2),1)'/sigma(i-1);
    
    v(i) = y(i) - z*tilde_mu((2*i-1):2*i,1); sigma(i) = z*Sigma((2*i-1):2*i,:)*z' + h*h';
    l((2*i-1):2*i,1) = S*Sigma((2*i-1):2*i,:)*z';
end

r = zeros(2*n,1); U = zeros(2*n,2); N = zeros(2*n,2);
% backwards recursions
for i = 1:n-1
    U(2*n-(2*i-1):2*n-2*(i-1),:) = S - l(2*n-(2*i-1):2*n-2*(i-1),1)*z/sigma(n-(i-1),1);
    r(2*n-(2*i+1):2*n-2*i,1) = z'*v(n-(i-1),1)/sigma(n-(i-1),1) + U(2*n-(2*i-1):2*n-2*(i-1),:)'*r(2*n-(2*i-1):2*n-2*(i-1),1);
    N(2*n-(2*i+1):2*n-2*i,:) = z'*z/sigma(n-(i-1),1) + U(2*n-(2*i-1):2*n-2*(i-1),:)'*N(2*n-(2*i-1):2*n-2*(i-1),:)*U(2*n-(2*i-1):2*n-2*(i-1),:);
end

% 
Ht = H';
hat_mu = zeros(2*n,1); epsi_c = zeros(n,1);
hat_mu((2*n-1):2*n,:) = tilde_mu(2*n-1:2*n,:);

%smoothed state vector hat_mu and smoothed disturbance
% i=n-1,...,3
for i=3:n-1
    hat_mu((2*i-1):2*i,1) = tilde_mu((2*i-1):2*i,1) + Sigma((2*i-1):2*i,:)*r(2*i+1:2*i+2,1);
    ht = h'*(v(i,1)-l((2*i-1):2*i,1)'*r((2*i-1):2*i,1))/sigma(i,1);
    epsi_c(i,1) =  Ht(1,:)*r((2*i-1):2*i,1) + ht(1,1);
end
epsi_c = epsi_c(3:end); 
hat_tau = zeros(n,1);
for i = 3:n
    hat_tau(i,1) = hat_mu(2*i-1,1); 
end
hat_tau = hat_tau(3:end);
%epsi_c(end,1) = y(end)-hat_tau(end); 
ht = h'*(v(end,1)-l(end-1:end,1)'*r(end-1:end,1))/sigma(end,1);
epsi_c(end,1) = sigma_c*ht(1,1);
Epsi_c = [epsi_c(1:end-1)*sigma_c;epsi_c(end)];
end
t(in,1)=toc;
end
figure(1)
plot(nnn,t,'g'); hold on;